#include "mc.h"

const unsigned int ADC_MASK[8] = {0x0000, 0x0002, 0x0001, 0x0004, 0x0002, 0x0001, 0x0004, 0x0000};
const unsigned int ADC_XOR[8] = {0x0000, 0x0000, 0xFFFF, 0x0000, 0xFFFF, 0x0000, 0xFFFF, 0x0000};
const unsigned int PWM_STATE[] = {0x0000, 0x2001, 0x2004, 0x0204, 0x0210, 0x0810, 0x0801, 0x0000}; //M3/M1,
const unsigned char ADC_BEMF_FILTER[64] ={0x00, 0x02, 0x04, 0x06, 0x08, 0x0A, 0x0C, 0x0E, 0x10, 0x12, 0x14, 0x16, 0x18, 0x1A, 0x1C, 0x1E,
    0x20, 0x22, 0x24, 0x26, 0x28, 0x2A, 0x2C, 0x2E, 0x01, 0x01, 0x01, 0x36, 0x01, 0x3A, 0x3C, 0x3E,
    0x00, 0x02, 0x04, 0x06, 0x08, 0x0A, 0x0C, 0x0E, 0x01, 0x01, 0x01, 0x16, 0x01, 0x1A, 0x1C, 0x1E,
    0x01, 0x01, 0x01, 0x26, 0x01, 0x2A, 0x2C, 0x2E, 0x01, 0x01, 0x01, 0x36, 0x01, 0x3A, 0x3C, 0x3E};

struct {
    unsigned RunMotor : 1;
    unsigned RampUpBEMF : 1;
    unsigned unused : 14;
} Flags;

struct {
    unsigned PhaseAOutput : 1;
    unsigned PhaseBOutput : 1;
    unsigned PhaseCOutput : 1;
    unsigned Commutation : 1;
    unsigned unused : 12;

} Comparator;

unsigned int PWMticks;
unsigned char CommState;
unsigned char ADCCommState;
unsigned char bemf_filter;
unsigned char ADC_bemf_filter;
unsigned char comm_done;

unsigned int u16BackEMFabc;
unsigned int u16MotorNeutralVoltage;
unsigned int u16MotorPhaseA;
unsigned int u16MotorPhaseB;
unsigned int u16MotorPhaseC;
unsigned int u16ComparatorOutputs;

unsigned int u16CommutationStatus;


unsigned char u8PoleCounter;

int timer3value;
int timer3avg;
int DesiredSpeed;
int ActualSpeed;
int SpeedError;
int SpeedIntegral;
int PILoopControllerOutput;
int u16DesiredPWMDutyCycle;
unsigned int Kps;
unsigned int Kis;
unsigned int u16CurrentPWMDutyCycle;

void InitMCClock(void) {
    OSCCONbits.NOSC = 1; //FRC with PLL
    OSCCONbits.CLKLOCK = 1; //CLOCK switching is disabled
    OSCCONbits.LPOSCEN = 0; //Disable Second OSC

    // Configure Oscillator to operate the device at 29491200Hz
    // Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
    // Fosc= 7.3728MHz*32/(2*2)= 58.9824Mhz for FRC input clock
    CLKDIVbits.DOZE = 0;
    CLKDIVbits.DOZEN = 0;
    CLKDIVbits.FRCDIV = 0;
    CLKDIVbits.PLLPOST = 0; // N1=2
    CLKDIVbits.PLLPRE = 0; // N2=2
    PLLFBD = 30; // M=32

    while (OSCCONbits.LOCK != 1) {
    }; // Wait for PLL to lock
}

void InitMC(void) {
    /*PWMs are outputs*/
    //LATBbits.LATB9 = 1; //PWM_EN, FIRE_EN in the MC Pictail Duaghter Board //TODO: brauchen wir das?
    //TRISBbits.TRISB9 = 0;

    LATBbits.LATB10 = 1; //PWM1H3, MC05 (Elektor-Leistungsplatine)
    TRISBbits.TRISB10 = 0;
    LATBbits.LATB11 = 0; //PWM1L3, MC04
    TRISBbits.TRISB11 = 0;
    LATBbits.LATB12 = 1; //PWM1H2, MC03
    TRISBbits.TRISB12 = 0;
    LATBbits.LATB13 = 0; //PWM1L2, MC02
    TRISBbits.TRISB13 = 0;
    LATBbits.LATB14 = 1; //PWM1H1, MC01
    TRISBbits.TRISB14 = 0;
    LATBbits.LATB15 = 0; //PWM1L1, MC00
    TRISBbits.TRISB15 = 0;


    Comparator.Commutation = 0;

    INTCON1bits.NSTDIS = 0; // Enabling nested interrupts	//TODO: brauchen wir das?
    InitMCPWM();
    InitADC10();
    InitTMR3();
    LATBbits.LATB9 = 0; //PWM_EN, FIRE_EN in the MC Pictail Duaghter Board //TODO: brauchen wir das?

    u8PoleCounter = 1;
    u16DesiredPWMDutyCycle = 0;
    DesiredSpeed = 100;
    ActualSpeed = 0;
    SpeedError = 0;
    SpeedIntegral = 0;
    Kps = 3000;
    Kis = 2;
    PILoopControllerOutput = 0;
    timer3value = 0;
    timer3avg = 0;

}

/******************************************************************************
 * Function:     main(void)
 *
 * Output:		None
 *
 * Overview:		Main funtion used to init the ADC, PWM and TIMER3 modules.
 *				It also inits the global variables used in the interrupts and
 *				PI controller.
 *				The main task excuted here is to start and stop the motor
 *				as well as setting the	ramp-up initial parameters to
 *				spin the motor
 *
 * Note:			None
 *******************************************************************************/
void mcMain(void) {
        //while (!S3); // wait for S3 button to be hit
        //while (S3) // wait till button is released
            //DelayNmSec(20);

        u16CurrentPWMDutyCycle = 1;
        u16DesiredPWMDutyCycle = 0;
        DesiredSpeed = 100;
        ActualSpeed = 0;
        SpeedError = 0;
        SpeedIntegral = 0;
        PILoopControllerOutput = 0;
        timer3value = 0;
        timer3avg = 0;


        PWM1CON1 = 0x0777; // enable PWM outputs
        DelayNmSec(1);
        Flags.RampUpBEMF = 1;
        Flags.RunMotor = 1; // indicationg the run motor condition



        while (Flags.RunMotor) // while motor is running
        {


            while (Flags.RampUpBEMF) {
                if (u16CurrentPWMDutyCycle < MIN_DUTY_CYCLE) // 1469 = 100% duty cycle
                {
                    u16CurrentPWMDutyCycle++;
                    DelayNmSec(1);
                } else {
                    //u16CurrentPWMDutyCycle = MIN_DUTY_CYCLE;
                    //u16DesiredPWMDutyCycle = MIN_DUTY_CYCLE;
                    Flags.RampUpBEMF = 0;
                    T3CONbits.TON = 1;
                }
            }

/*
            if (S3) // if S5 is pressed
            {

                PWM1CON1 = 0x0700; // disable PWM outputs
                P1OVDCON = 0x0000; // overide PWM low.
                Flags.RampUpBEMF = 0;
                Flags.RunMotor = 0; // reset run flag
                u16CurrentPWMDutyCycle = 1;

                while (S3) // wait for key release
                    DelayNmSec(20);
            }
*/

        }//end of motor running loop
}//end of main function

/******************************************************************************
 * Function:     _ADCInterrupt(void)
 *
 * Output:		None
 *
 * Overview:		ADC interrupt used to measure the BEMF signals, recontruct
 * 				the Motor Virtual Neutral Point and compare the BEMF signals
 *				agains the neutral point reference to detect the zero-crossing
 *				event
 *
 * Note:			None
 *******************************************************************************/
void __attribute__((__interrupt__, auto_psv)) _ADC1Interrupt(void) {

    //DesiredSpeed = ADC1BUF0;
    u16MotorPhaseA = ADC1BUF1;
    u16MotorPhaseC = ADC1BUF2;
    u16MotorPhaseB = ADC1BUF3;
    u16MotorNeutralVoltage = (u16MotorPhaseA + u16MotorPhaseB + u16MotorPhaseC) / 3;
    AD1CON1bits.DONE = 0;
    IFS0bits.AD1IF = 0;

    /********************* ADC SAMPLING & BMEF signals comparison ****************/
    if (u16MotorPhaseA > u16MotorNeutralVoltage) {
        Comparator.PhaseAOutput = 1;
    }
    else {
        Comparator.PhaseAOutput = 0;
    }
    if (u16MotorPhaseB > u16MotorNeutralVoltage) {
        Comparator.PhaseBOutput = 1;
    }
    else {
        Comparator.PhaseBOutput = 0;
    }
    if (u16MotorPhaseC > u16MotorNeutralVoltage) {
        Comparator.PhaseCOutput = 1;
    }
    else {
        Comparator.PhaseCOutput = 0;
    }

    u16ComparatorOutputs = (Comparator.PhaseCOutput << 2) & 0x0007;
    u16ComparatorOutputs |= (Comparator.PhaseBOutput << 1);
    u16ComparatorOutputs |= Comparator.PhaseAOutput;

}

/******************************************************************************
 * Function:     _PWMInterrupt(void)
 *
 * Output:		None
 *
 * Overview:		PWM relod interrupt used to filter the BEMF signals using the
 *				majority detection filter to detect a valid xero-crossing event
 *				if a valid zero-cressignevent was detected then Commutate.
 *				This funtion also includes the start-upt sequence for detecting
 *				the intial rotor position
 *
 * Note:			None
 *******************************************************************************/
void __attribute__((__interrupt__, auto_psv)) _MPWM1Interrupt(void) {

    if ((u16ComparatorOutputs^ADC_XOR[ADCCommState]) & ADC_MASK[ADCCommState]) {
        ADC_bemf_filter |= 0x01;
    }

    if (u16CurrentPWMDutyCycle > 160)
        P1SECMPbits.SEVTCMP = u16CurrentPWMDutyCycle >> 1;
    else if (u16CurrentPWMDutyCycle > 76)
        //P1SECMPbits.SEVTCMP =48;
        P1SECMPbits.SEVTCMP = u16CurrentPWMDutyCycle >> 3;
    else
        P1SECMPbits.SEVTCMP = 0;


    ADC_bemf_filter = ADC_BEMF_FILTER[ADC_bemf_filter];
    if (ADC_bemf_filter & 0b00000001) {
        Commutate();
        Comparator.Commutation = ~Comparator.Commutation;
        u16CommutationStatus = Comparator.Commutation;
    }
    //Ramp-up period to detect the rotor position
    if (Flags.RampUpBEMF)
        P1DC1 = P1DC2 = P1DC3 = u16CurrentPWMDutyCycle;
    if (++PWMticks > MAX_PWMticks)
        Commutate();

    IFS3bits.PWM1IF = 0;

}

/******************************************************************************
 * Function:     Commutate(void)
 *
 * Output:		None
 *
 * Overview:		Here is where the motor commutation occurs, this funtion also
 *				computates the rotor speed using the TIMER3
 *
 * Note:			None
 *******************************************************************************/
void Commutate(void) {
    // The code below is uses TMR3 to calculate the speed of the rotor
    if (ADCCommState == INDEX) // has the same position been sensed?
        if (u8PoleCounter++ >= POLEPAIRS) // has one mech rev elasped?
        { // if yes then read timer 3
            T3CONbits.TON = 0; // Stop TIMER3
            timer3value = TMR3;
            TMR3 = 0;
            timer3avg = ((timer3avg + timer3value) >> 1);

            if (!Flags.RampUpBEMF)
                SpeedPILoopController();

            u8PoleCounter = 1;
            T3CONbits.TON = 1; // Start TIMER3

        }

    //Commutation
    ADC_bemf_filter = 0;
    if (++ADCCommState > 6)
        ADCCommState = 1;
    P1OVDCON = PWM_STATE[ADCCommState];

    PWMticks = 0;

}

/******************************************************************************
 * Function:     SpeedPILoopController(void)
 *
 * Output:		None
 *
 * Overview:		When the CLOSELOOPMODE macro is defined the compiler adds
 *				the PI controller for the speed, if CLOSELOOPMODE is not defined
 *				then the motor operates in open loop mode. The Kp and Ki
 *				paratemers were determined using the HURST MOTOR shipped with
 *				the MCLV board. This values should be modified according to
 *				the motor specs.
 *
 * Note:			None
 *******************************************************************************/
/******************************************************************************/
/***************          CLOSE LOOP MODE              ************************/
/******************************************************************************/
#ifdef CLOSELOOPMODE

void SpeedPILoopController(void) {

    //For the HURST motor the Timer3Avg values
    //TIMER3 counts = 9562 AT 7.5% dutycycle (P1DC1 = 109)
    //TIMER3 counts = 714  AT 100% dutycycle (P1DC1 = 1469)
    //RPSelectrical = 19-261Hz
    //RPMelctrical = 1179-15669RPM

    //Normalizing TIMER3 counts to electrical RPM
    ActualSpeed = (SPEEDMULT / timer3avg)*60;
    //ADC POT RANGE 0-1024, 1024*16 = MAXSPEED in electrical RPM
    DesiredSpeed = DesiredSpeed << 4;
    //Calculating Error
    SpeedError = DesiredSpeed - ActualSpeed;
    SpeedIntegral += SpeedError;
    //Dividing PI output by PI controller gain (2exp14)
    PILoopControllerOutput = (((long) Kps * (long) SpeedError + (long) Kis * (long) SpeedIntegral) >> 14);
    //Divided by 11 to normalize the PI output to PWM PDCx values
    u16DesiredPWMDutyCycle = u16DesiredPWMDutyCycle + (PILoopControllerOutput / 11);
    // Max and Min PWM duty cycle limits
    if (u16DesiredPWMDutyCycle < MIN_DUTY_CYCLE) {
        u16DesiredPWMDutyCycle = MIN_DUTY_CYCLE;
        SpeedIntegral = 0;
    }
    if (u16DesiredPWMDutyCycle > MAX_DUTY_CYCLE) {
        u16DesiredPWMDutyCycle = MAX_DUTY_CYCLE;
        SpeedIntegral = 0;
    }

    P1DC1 = u16DesiredPWMDutyCycle;
    P1DC2 = u16DesiredPWMDutyCycle;
    P1DC3 = u16DesiredPWMDutyCycle;
    u16CurrentPWMDutyCycle = u16DesiredPWMDutyCycle;

}

/******************************************************************************/
/***************          OPEN LOOP MODE               ************************/
/******************************************************************************/
#else 

void SpeedPILoopController(void) {
    //PWM duty cycle = pot value *2
    u16DesiredPWMDutyCycle = DesiredSpeed << 1;
    u16CurrentPWMDutyCycle = u16DesiredPWMDutyCycle;

    if (u16CurrentPWMDutyCycle != u16DesiredPWMDutyCycle) {
        if (u16CurrentPWMDutyCycle < u16DesiredPWMDutyCycle)
            u16CurrentPWMDutyCycle++;
        if (u16CurrentPWMDutyCycle > u16DesiredPWMDutyCycle)
            u16CurrentPWMDutyCycle--;
    }

    if (u16CurrentPWMDutyCycle < MIN_DUTY_CYCLE)
        u16CurrentPWMDutyCycle = MIN_DUTY_CYCLE;
    if (u16CurrentPWMDutyCycle > MAX_DUTY_CYCLE)
        u16CurrentPWMDutyCycle = MAX_DUTY_CYCLE;

    P1DC1 = u16CurrentPWMDutyCycle;
    P1DC2 = u16CurrentPWMDutyCycle;
    P1DC3 = u16CurrentPWMDutyCycle;

}
#endif

/******************************************************************************
 * Function:     InitADC10(void)
 *
 * Output:		None
 *
 * Overview:		Initializes the ADC module to operate in sumoultaneous mode
 *				sampling terminals AN0,AN1, AN2, AN3 using MUX A. The ADC channels are
 *				assigned as follows in the MCLV board
 *				CH0->AN3  (POT)
 *				CH1->AN0 (HALLA, PHASE A)
 *				CH2->AN1 (HALLB, PHASE B)
 *				CH3->AN2 (HALLC, PHASE C)
 * 				ADC is sync with the PWM. ADC is conversion is triggered
 *				everytim a PWM relod event occurs. Tadc = 84.75 nSec.
 *				ADC resulting samples are formatted as unsigned 10-bits
 *				left-justified
 *
 * Note:			None
 *******************************************************************************/
void InitADC10(void) {

    AD1PCFGL = 0x0003; //Port pin multiplexed with AN0-AN1 in Digital mode
    //Port pin multiplexed with AN2-AN5 in Analog mode



    //AD1CON1 = 0x006C; //ADC is off
    //Continue module operation in Idle mode
    //10-bit, 4-channel ADC operation
    //Data Output Format bits Integer (0000 00dd dddd dddd)
    //011 = Motor Control PWM interval ends sampling and starts conversion
    //Samples CH0, CH1, CH2, CH3 simultaneously when CHPS<1:0> = 1x
    //Sampling begins immediately after last conversion SAMP bit is auto-set.
    AD1CON1 = 0;
    AD1CON1bits.SSRC = 0b011;   //Motor Control PWM interval ends sampling and starts conversion
    AD1CON1bits.SIMSAM = 1;     //Samples CH0, CH1, CH2, CH3 simultaneously when CHPS<1:0> = 1x
    AD1CON1bits.ASAM = 1;       //Sampling begins immediately after last conversion SAMP bit is auto-set.

    AD1CHS123 = 0x0001; //MUX B CH1, CH2, CH3 negative input is VREF-
    //MUX B CH1 positive input is AN0, CH2 positive input is AN1, CH3 positive input is AN2
    //MUX A CH1, CH2, CH3 negative input is VREF-
    //MUX A CH1 positive input is AN3, CH2 positive input is AN4, CH3 positive input is AN5

    //AD1CHS0 = 0x0602; //MUX B Channel 0 negative input is VREF-
    //MUX B Channel 0 positive input is AN6
    //MUX A Channel 0 negative input is VREF-
    //MUX A Channel 0 positive input is AN2
    AD1CHS0 = 0;

    AD1CSSL = 0x0000; //Skip all ANx channels for input scan

    AD1CON3 = 0x0002; //ADC Clock derived from system clock
    //Autosample time time bits = 0 TAD sinjce PWM is controlling sampling time
    //TAD = 3*TCY, TAD = 101.7 nSec


    AD1CON2 = 0x0300; //ADREF+ = AVDD ADREF- = AVSS
    //Do not scan inputs
    //1x = Converts CH0, CH1, CH2 and CH3
    //A/D is currently filling buffer 0x0-0x7
    //Interrupts at the completion of conversion for each sample/convert sequence
    //Always starts filling buffer from the beginning
    //Always uses channel input selects for Sample A

    AD1CON1bits.DONE = 0;
    IPC3bits.AD1IP = 5;
    IFS0bits.AD1IF = 0;
    IEC0bits.AD1IE = 1;
    AD1CON1bits.ADON = 1;

}

/******************************************************************************
 * Function:     InitMCPWM(void)
 *
 * Output:		None
 *
 * Overview:		Initializes the PWM module to operate in center-aligned mode
 *				at 20KHz. PWM terminals are configured in independent mode.
 *				PWM time base is 67.8 nSec.
 *				PDCx value range is 0-1464 for 0%-100% duty cycle
 *				ADC reload time is variable according to the PWM dutycycle
 *
 *
 * Note:			None
 *******************************************************************************/
void InitMCPWM(void) {
    P1TPER = ((FCY / FPWM) / 2 - 1);
    //FCY  29491200...FRC w/PLL x16
    //FPWM 20KHz PWM Freq
    // MAX_DUTY_CYCLE = 1469
    // 50% duty cycle = 734


    P1TCONbits.PTSIDL = 1; // PWM time base halted in CPU IDLE mode
    P1TCONbits.PTOPS = 0; // PWM time base 1:1 postscale
    P1TCONbits.PTCKPS = 0; // PWM time base 1:1 prescale
    P1TCONbits.PTMOD = 2; // Center Aligned with single interrupt mode per PWM period

    PWM1CON1 = 0x0700; // disable PWMs
    P1OVDCON = 0x0000; // allow control using OVD

    P1SECMPbits.SEVTDIR = 0; // trgger ADC when PWM counter is in upwards dir
    //....Tad=84.77, Tpwm=67.816
    P1SECMPbits.SEVTCMP = 0; // generates a trigger event for the ADC
    // when PWM time base is counting upwards
    // just before reaching the PTPER value
    // causing a sampling in the middle of the
    // pulse

    //kP1FLTACON = 0x008F;		// The PWM output pin is driven Inactive on
    // an external fault input event
    // The Fault A input pin latches all control pins to 0
    // PWMxH4-1/PWMxL4-1 pin pair is controlled by
    // Fault Input A


    PWM1CON2 = 0x0000; // 1:1 postscale values
    // Updates to the active PxDCy registers
    // are sync to the PWM time base
    // Output overrides via the PxOVDCON register occur
    // on the next TCY boundary
    // Updates from duty cycle and period buffer registers
    // are enabled

    IPC14bits.PWM1IP = 4; // PWM Interrupt Priority 4
    IFS3bits.PWM1IF = 0; // clear
    IEC3bits.PWM1IE = 1;


    P1TCONbits.PTEN = 1;
}

/******************************************************************************
 * Function:     InitTMR3(void)
 *
 * Output:		None
 *
 * Overview:		Initializes the TIMER3 module to operate in free-running
 *				upcounting mode. The TIMER3 time base is Tcy/256 = 8.68uSec.
 *				This timer is used to calculate the motor speed
 *
 * Note:			None
 *******************************************************************************/
void InitTMR3(void) { // Tcy = 33.908 nSec
    TMR3 = 0;
    PR3 = 0xFFFF;
    T3CON = 0x0030; // internal Tcy*256 clock = 8.68 uSec
}

/******************************************************************************
 * Function:     DelayNmSec(unsigned int N)
 *
 * Output:		None
 *
 * Overview:		Delay funtion used for push buttons debounce loop and for the
 *				motor start-up sequence
 *
 * Note:			None
 *******************************************************************************/
void DelayNmSec(unsigned int N) {
    unsigned int j;
    while (N--)
        for (j = 0; j < MILLISEC; j++);
}

